/*
 * CameraStateValidityCheckerM.cpp
 *
 *  Created on: Mar 22, 2013
 *      Author: ohnozzy
 */

#include "CameraStateValidityCheckerM.h"
#include "flyingcamera/conversion/Conversion.h"
#include <iostream>
#include <utility>
namespace fb = flyingcamera::base;

bool flyingcamera::base::moving::CameraStateValidityCheckerM::isValid(const ompl::base::State *state) const
{
	const fb::CameraSpace::StateType *cstate = state->as<fb::CameraSpace::StateType>();
	const ompl::base::SO3StateSpace::StateType& rotation=cstate->rotation();
    double t=cstate->getT();
    //double t = 0.553213;
    double x, y, Yaw;
    fcl::Quaternion3f fcl_r;
    /*fcl_r.getW () = 0.727662;
    fcl_r.getX () = 0.606443;
    fcl_r.getY () = 0.223499;
    fcl_r.getZ () = -0.229746;*/
    fcl_r.getW () = rotation.w;
    fcl_r.getX () = rotation.x;
    fcl_r.getY () = rotation.y;
    fcl_r.getZ () = rotation.z;
    fcl::Transform3f rtf(fcl_r);
    path->getLocation(t, x, y, Yaw);
    flyingcamera::conversion::moveUpandTranslate(rtf, robot.lz, x, y);
    fcl::Matrix3f rm;
    fcl_r.toRotation(rm);
    //Prevent the camera going under the ground
    if(0.5*rm(2,2)*robot.lz+rtf.getTranslation()[2]<0)
    	return false;
    bool isValid = true;

    for(unsigned i=0; i<obstacle.size();i++){
    	fcl::CollisionRequest collisionRequest;
    	fcl::CollisionResult collisionResult;
    	const fcl::CollisionGeometry* o=obstacle[i];
        const fcl::Transform3f& otf=obstacle_tf[i];

        std::size_t collision_size=fcl::collide(&robot, rtf, o, otf,collisionRequest,collisionResult);
        if(collision_size>0){
        	isValid = false;
        	break;
        }
    }

    //std::cout<<"near obstacles: "<<m_obstacles.size()<<std::endl;
    if (isValid)
    {
    	for(unsigned i=0; i<m_obstacles.size();i++){
    		fcl::CollisionRequest collisionRequest;
    	    fcl::CollisionResult collisionResult;
    	    const fcl::CollisionGeometry* o = m_obstacles[i];
    	    const std::pair< double, double > v = m_obstacles_velocity[i];
    	    fcl::Transform3f tf = m_obstacles_tf[i];
    	    fcl::Vec3f tr = tf.getTranslation();

    	    double x, y, z;
    	    x = tr.data[0] + v.first * (t - time_)/delta_t;
    	    y = tr.data[1] + v.second * (t - time_)/delta_t;
    	    z = tr.data[2];
    	    fcl::Vec3f v3f(x, y, z);
    	    //fcl::Vec3f v3f(0.55, 0.519278, 0.1);
    	    fcl::Transform3f otf(v3f);
    	    std::size_t collision_size=fcl::collide(&robot, rtf, o, otf,collisionRequest,collisionResult);
    	    if(collision_size>0)
    	    {
    	       	return false;
    	    }
    	}
    }

    return isValid;
}

void flyingcamera::base::moving::CameraStateValidityCheckerM::addObstacle(fcl::CollisionGeometry* const o, const fcl::Transform3f& tf){
	obstacle.push_back(o);
	obstacle_tf.push_back(tf);
}
void flyingcamera::base::moving::CameraStateValidityCheckerM::addMObstacle(fcl::CollisionGeometry* const obstacle, const fcl::Transform3f&  tf, const std::pair< double, double > velocity)
{
	m_obstacles.push_back(obstacle);
	m_obstacles_velocity.push_back(velocity);
	m_obstacles_tf.push_back(tf);
}
void flyingcamera::base::moving::CameraStateValidityCheckerM::removeMObstacles()
{
	m_obstacles.clear();
	m_obstacles_tf.clear();
	m_obstacles_velocity.clear();
}
void flyingcamera::base::moving::CameraStateValidityCheckerM::setStartTime(double time)
{
	time_ = time;
}
double flyingcamera::base::moving::CameraStateValidityCheckerM::getStartTime()
{
	//std::cout<<"Start Time: "<<time_<<std::endl;
	return time_;
}


